#include "v4l2api.h"



V4l2Api::V4l2Api(const char *devname)
{
    status = false;
    //1.打开设备/dev/video0
    this->fd = open(devname, O_RDWR);
    if(fd < 0)
    {
        perror("open error");
    }
}

V4l2Api::~V4l2Api()
{
    delete []this->mptr;
    delete []this->mptrsize;
    //munmap();
    //close();
}

int V4l2Api::getSize()const
{
    return this->size;
}

void V4l2Api::getFrame(unsigned char *data, int size)
{
    //从队列中获取一个缓冲区
    struct v4l2_buffer vbuf;
    vbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(fd, VIDIOC_DQBUF, &vbuf);
    if(ret < 0)
    {
        perror("s--dqbuf");
    }
    memcpy(data, mptr[vbuf.index], vbuf.length>size?size:vbuf.length);
    //把缓冲区放回队列
    ret = ioctl(fd, VIDIOC_QBUF, &vbuf);
    if(ret < 0)
    {
        perror("s--qbuf");
    }
}

void V4l2Api::setImageFormat(int w, int h, int fmt)
{
    this->width = w;
    this->height = h;
    this->format = fmt;
    //设置采集格式
    struct v4l2_format vformat;
    vformat.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    //初始宽，高， 格式
    vformat.fmt.pix.width = this->width;
    vformat.fmt.pix.height = this->height;
    vformat.fmt.pix.pixelformat = this->format;
    int ret = ioctl(fd, VIDIOC_S_FMT, &vformat);
    if(ret < 0)
    {
        perror("set format error");
        return ;
    }

    //清空
    memset(&vformat.fmt, 0, sizeof(vformat.fmt));
    ret = ioctl(fd, VIDIOC_G_FMT, &vformat);
    //输出
    printf("w:%d\nh:%d\n", vformat.fmt.pix.width, vformat.fmt.pix.height);
    printf("format:%s\n", (char*)&vformat.fmt.pix.pixelformat);
}

void V4l2Api::initVideo(int w, int h, int fmt, int count)
{
    setImageFormat(w, h, fmt);
    this->cout = count;
    //申请缓冲区
    struct v4l2_requestbuffers  reqbuffer;
    reqbuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    reqbuffer.count = this->cout;
    reqbuffer.memory = V4L2_MEMORY_MMAP;
    int ret = ioctl(fd, VIDIOC_REQBUFS, &reqbuffer);
    if(ret < 0)
    {
        perror("request error");
    }

    this->mptr = new unsigned char*[this->cout];
    this->mptrsize = new unsigned int[this->cout];
    //映射缓冲区
    for(int i=0; i<this->cout; i++)
    {
        //查询
        struct v4l2_buffer vbuffer;
        vbuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        vbuffer.index = i;
        vbuffer.memory = V4L2_MEMORY_MMAP;
        int  ret = ioctl(fd, VIDIOC_QUERYBUF, &vbuffer);
        if(ret < 0)
        {
            perror("query error");
        }

        //映射
        this->mptr[i] = (unsigned char*)mmap(NULL, vbuffer.length, PROT_READ|PROT_WRITE,MAP_SHARED, fd, vbuffer.m.offset);
        this->mptrsize[i] = vbuffer.length;
        //归还（清除）
        ret = ioctl(fd, VIDIOC_QBUF, &vbuffer);
        if(ret < 0)
        {
            perror("qbuf error");
        }
    }
    this->size = this->mptrsize[0];
}

void V4l2Api::yuv422ToRgb(unsigned char *yuv, int w, int h, unsigned char *rgb)
{
    char yuvbuffer[4]={0};
    for(int i=0; i<h; i++)
    {
        for(int j=0; j<w/2; j++)
        {
            //把4字节还原为yuv完整数据
            unsigned char y0,u0,v0,y1,u1,v1;
            y0=yuv[0];
            u0=yuv[1];
            v0=yuv[3];
            y1=yuv[2];
            u1=yuv[1];
            v1=yuv[3];
            //R = Y + 1.402 * (V-128)
            //G = Y - 0.34414 * (U - 128) - 0.71414 * (V - 128)
            //B = Y + 1.772 * (U - 128)
            //把俩个yuv像素转rgb像素
            unsigned int  rgbo[6];
            rgbo[0]=y0+1.402*(v0-128);
            rgbo[1]=y0-0.34414*(u0-128)-0.71414*(v0-128);
            rgbo[2]=y0+1.772*(u0-128);

            rgbo[3]=y1+1.402*(v1-128);
            rgbo[4]=y1-0.34414*(u1-128)-0.71414*(v1-128);
            rgbo[5]=y1+1.772*(u1-128);

            for(int i=0; i<6; i++)
            {
                rgb[i]= rgbo[i]>255?255:rgbo[i];
                rgb[i]= rgbo[i]<0?0:rgbo[i];
            }
            rgb+=6;
            yuv+=4;
        }
    }
}

void V4l2Api::start()
{
    if (status) return;
    //映射缓冲区
    for(int i=0; i<this->cout; i++)
    {
        //查询
        struct v4l2_buffer vbuffer;
        vbuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        vbuffer.index = i;
        vbuffer.memory = V4L2_MEMORY_MMAP;
        int  ret = ioctl(fd, VIDIOC_QUERYBUF, &vbuffer);
        if(ret < 0)
        {
            perror("query error");
        }
        //归还（清除）
        ret = ioctl(fd, VIDIOC_QBUF, &vbuffer);
        if(ret < 0)
        {
            perror("qbuf error");
        }
    }

    //开启采集
    int argp=V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(this->fd, VIDIOC_STREAMON, &argp);
    if(ret < 0)
    {
        perror("start error");
    }
    status = true;
}

void V4l2Api::stop()
{
    if (!status) return;
    //停止采集
    int argp=V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(this->fd, VIDIOC_STREAMOFF, &argp);
    if(ret < 0)
    {
        perror("stop error");
    }
    status = false;
}
